import rclpy
from rclpy.node import Node
from std_msgs.msg import Int16
from sls_laserdis.high import SerialPort

class LaserDistMeasure(Node):
    def __init__(self):
        super().__init__('laser_dist_measure_node')
        self.pub = self.create_publisher(Int16,'/sls/sensors/height_laser/out/dist_measure',2)
        self.timer = self.create_timer(0.1, self.timeCallBack)
        self.serial_port = SerialPort('/dev/ttyACM0', 115200)
        # 打开串口
        self.serial_port.open()

    def timeCallBack(self):
        data = Int16()
        H_data = self.serial_port.read_data()
        if H_data > 8000:
            data.data = -1
        else:
            data.data = H_data
        self.pub.publish(data) 
        print(data.data)
        

def main(args = None):
    rclpy.init(args=args)
    node = LaserDistMeasure()
    rclpy.spin(node)
    node.destroy_node(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()